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ABSTRACT 


A  sequential  Extended  Kalman  Filter  and  Smoothing 
routine  was  developed  to  provide  real  time  estimates  of 
torpedo  position  and  depth  on  the  three  dimensional  under¬ 
water  tracking  range  at  the  Naval  Torpedo  Station,  Keyport, 
Washington.  Inputs  to  the  routine  were  acoustic  pulse 
transit  times  from  the  target  to  receiving  array  elements 
which  are  non-linear  functions  of  the  position  coordinates. 
These  inputs  were  linearized  and  the  filter  gains  and 
filtered  estimates  calculated  on-line.  By  using  a  smoothing 
subroutine,  all  past  filtered  estimates  were  smoothed. 

Tests  were  conducted  using  simulated  torpedo  trajectories 
that  traversed  multiple  hydrophone  arrays.  It  was  found 
that  filter  performance  was  dependent  on  system  noise  and 
the  distance  the  torpedo  was  from  the  hydrophone  array  and 
the  smoothed  estimates  of  states  were  better  than  or  equal 
to  the  filtered  estimates. 
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I.  INTRODUCTION 


The  NUWES  at  Keyport,  Washington  currently  operates  two 
three-dimensional  (3-D)  underwater  tracking  range  utilizing 
a  sonar  transmitter  installed  in  the  torpedo  to  be  tracked. 
The  transmitter  is  synchronized  with  a  master  clock.  Timed 
acoustic  pulses  are  received  by  bottom  mounted  hydrophone 
arrays  and  then  relayed  via  cable  to  a  computer  at  the 
observation  site.  The  computer  calculates  the  positional 
coordinates  of  the  torpedo  and  plots  its  trajectory  through 
the  water. 

The  measured  data,  which  consists  of  the  elapsed  time 
from  transmission  of  a  pulse  until  its  receipt  at  the 
hydrophone  array,  is  corrupted  with  noise  due  to  the 
combined  effects  of  environmental  factors  and  measurement 
instruments . 

These  noisy  tracks  are  later  analyzed,  and  measurements 
judged  most  inaccurate  on  the  basis  of  total  track  statis¬ 
tics  are  removed  in  order  to  obtain  a  smooth  representation 
of  the  track. 

An  opportunity  exists  for  expanding  the  capability  of 
the  system  by  applying  a  real  time  Kalman  Filter  and  post 
test  Smoothing  routine  which  can  take  as  an  input  the 
transit  times  of  the  acoustic  pulses,  and  produce  the  best 


8 


estimate  of  the  position  of  the  tracked  object  at  a 
particular  time.  Previous  research  in  this  area  C 3 3  and 
[4],  revealed  that  a  Kalman  filter  utilizing  a  sequential 
estimation  approach  was  desirable. 

The  intention  is  to  develop  and  test  a  sequential  Kalman 
filter  and  smoothing  algorithm  that  can  be  interfaced  with 
the  current  underwater  range  system. 


II .  DESCRIPTION  OF  RANGE  TRACKING  GEOMETRY 

The  hydrophone  array,  consisting  of  four  independent 
elements,  defines  an  orthogonal  coordinate  system  in  which 
transit  time  measurements  are  made.  As  shown  in  Figure  1, 
four  hydrophones  X,  Y,  Z,  and  C  are  on  four  adjacent 
vertices  separated  by  a  distance  d,  along  the  edge  of  the 
cube.  The  origin  of  the  array  coordinates  is  at  the  center 
of  the  cube  with  the  orthogonal  coordinates  parallel  to  its 
edge.  Positional  information  is  computed  from  the  transit 
times  of  a  periodic  synchronous  acoustic  signal  travelling 
from  the  torpedo  to  the  four  hydrophones  on  the  array.  The 
range  measures  the  tracked  torpedo’s  position  every  1.31 
seconds  to  an  accuracy  that  is  typically  within  3  to  30 
feet.  A  more  detailed  description  of  the  range  tracking 
capability  is  described  in  [23. 
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Figure  1.  Geometry  of  a  Tracking  Array 
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III.  THEORY 


A.  THE  EXTENDED  KALMAN  FILTER 

Since  the  transit  times  were  readily  available  and  are 
nonlinear  functions  of  position,  these  equations  can  be 
linearized  and  Kalman  filter  theory  applied  using  the 
extended  Kalman  filter.  This  procedure  produces  a  real-time 
system,  filtering  on  the  transit  times  Tc,  Tx ,  Ty  and  Tz, 
without  the  necessity  of  converting  these  times  to 
positions . 

For  the  three-dimensional  location  problem  three 
position  states  (x,  y,  z)  and  two  velocity  states  (vx,  vy) 
specify  target  motion.  The  discrete  linear  and  nonlinear 
observation  equations  are  given  by 


x(k  +  1)  =  *  •  x (k)  +  r  •  w(k) 


(3.1) 


and 


z(k)  s  h(x(k),  k)  +  v(k) 


(3 


2) 


In  these  equations  *  and  r  are  constant  matrices  and  h  is  a 
nonlinear  function  of  the  state  variable  x.  w(k)  is  plant 
excitation  noise  and  v^(k)  is  measurement  noise.  The  plant 
noise  and  measurement  noise  are  assumed  uncorrelated  (white) 
with  zero  mean.  That  is, 


ECw(k)  •  wA(J)]  s  Q*(k)4 


kJ 

and 

E[v(k)  •  vT(3)]  =  R(k)«kj 

with 

5kj  =  1  ,  k  =  J 
=  0  ,  k  i  J 

In  order  to  apply  the  linear  filter  equation  (3.2)  is 
expanded  in  a  Taylor  series  about  the  best  estimate  of  the 
state  at  that  time  and  only  the  first-order  terms  are  kept 
Equation  (3.2)  gives 

z(k)  s  H(k)  •  £(k)  +  v(k)  (3. 

where 


H(k) 


ah 

3  X 


x  ’  (k)  =  x(k/k-l) 


C  3 . 3 


£(k/k-1)  is  a  predicted  value  of  the  state  before  the  kth 
measurement . 

A  state  error  vector  is  defined  by 
?(k/k)  s  £(k/k)  -  x(k)  , 


and  a  predicted  state  error  vector  is  defined  by 


x(k/k-1)  s  x(k/k-1)  -  x(k)  . 


The  covariance  of  state  error  matrix  is  defined  by 
P(k/k>  =  E[x(k/k)  •  xT(k/k)]  , 

and  the  predicted  covariance  of  state  error  matrix  is  given 

by 

P(k/k-1 )  =  E[x(k/k-1)  •  xT0</k-1)]  . 

The  state  excitation  matrix  is  given  by 
Q(k)  s  r (k)  E[w(k)  •  wT(k)]  .  rT(k) 
and  the  measurement  noise  covariance  matrix  is 
ROO  =  E[ v(k)  •  v T ( k )  ]  . 

The  Kalman  filter  equations  are  given  by  [1]: 


P(k+1/k)  =  *P(k/k) *T  +  Q(k)  (3.4a) 
G(k)  =  P(k/k-1)HT(k)[H(k)*P(k/k-1)HT(k)  +  R(k)]"1  (3.4b) 
P(k)  a  [I  -  G(k)H(k) ]  P( k/k-1 )  (3.4c) 
x(k+l/k)  s  *  £(k/k)  (3.4d) 
z(k/k-1)  =h(x(k/k-1),  k)  (3.4e) 
x(k)  s  x(k/k-1)  +  G(k)[z(k)  -  z(k/k-1)]  (3.4f) 
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) 

I 


The  Q  matrix  serves  not  only  to  allow  i  cr  maneuvering 
but  also  to  account  for  any  model  inaccuracies,  that  is,  any 
discrepancies  between  the  true  action  of  the  torpedo  and  its 
characterization  by  Equation  (3.1).  The  Q  also  serves  to 
prevent  the  gain  matrix  GOO  from  approaching  zero  by  always 
insuring  uncertainty  in  the  predicted  covariance  of  error 
matrix  P(k+1/k) . 

B.  OPTIMAL  LINEAR  SMOOTHING 

Smoothing  is  a  non-real-time  data  processing  scheme  that 
uses  all  measurements  between  0  and  T  to  estimate  the  state 
of  a  system  at  certain  time  t,  where  0  <  t  (  T.  The 
smoothed  estimate  of  jc C t )  based  on  all  the  measurements 
between  0  and  T  is  denoted  by 

Smoother  error  covariance  is  denoted  by  P(t/T)  and 
P(t/T)  <  P(t)  means  that  the  smoothed  estimate  of  jc(t)  is 
always  better  than  or  equal  to  its  filtered  estimate.  This 
is  shown  graphically  in  Figure  2. 

Several  forms  of  the  smoothing  equations  may  be  derived. 
One  is  the  Rauch-Tung-Str iebel  form,  which  was  used  in  our 
particular  case  with  the  discrete-time  expressions 
summarized  as  follows  [1]: 

X(k/N)  *  2(k/k)  +  AkC£(k+1/N)  -X(k+1/k)]  (3.5a) 
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Figure  2.  Advantage  of  Performing  Optimal  Smoothing  [1] 

where 

Ak  =  P(lc/k)i(k)TP(’K+1/k)"1  for  k  =  N-1 

P(k/N)  =  P(k/k)  +  Ak[PCk+1/N)  -  P(k+1/k) ] A^1  (3.5b) 


also  for  k  i  M-1 . 

In  these  equations  £(k/N)  is  smoothed  State  Estimate  and 

At 

P(k/N)  is  Error  Covariance  Matrix  Propagation. 


16 


IV.  PROBLEM  DEFINITION  -  TORPEDO  TRACKING  WITH  THE  EXTENDED 

KXntiCH  FILTEYTtfD'  "O'PTI'MAL'  'SMOOTHING - 


A.  FILTER  EQUATIONS 

In  the  torpedo  tracking  problem,  the  non-linear 
observations  are  the  four  independent  transit  times  from  the 
tracked  object  to  the  hydrophones,  T„ ,  T  ,  T  and  T  .  Thus 

c  a  y 

the  non-linear  measurement  matrix  z(k)  is  defined  as: 


—  — 

— 

v*) 

^[x(k)+d/2)2+(y(k)+d/2)2+(z(k)+d/2)2]1/2+v(k) 

TXU) 

Y]jxC(x(k)-d/2)2+(y(k)+d/2)2+(z(k)+d/2)2]1/2+v(k) 

Ty(k) 

y^-[x (k)+d/2)^+y(k)-d/2)2+(z(k)+d/2)2] 1/2+v(k) 

Tz(k) 

YYlC(  x(k)+d/2)2+(y(k)+d/2)2+(z(k)-d/2)2]  1/2+v(k) 

_  — 

(4.1) 


The  measurement  noises,  v(k)*s,  are  assumed  to  be  zero-mean 
and  independent  with  a  covariance  matrix 


R(k)  = 


0 


(4.2) 
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Equation  (3* 3a)  can  be  used  to  give  the  linearized 
observation  matrix.  When  the  derivatives  are  taken  and 
evaluated  at  the  predicted  state  values  x(k/k-1)  s  x’(k)  the 
result  is 


H(k)s 


Wl 


where 


x  * (k)+d/2 

0 

y '  (k)+d/2 

0 

z* (k)+d/2 

“  ~  ‘OEM  1 

DENI  * 

DENT 

x' (k)-d/2 

— m?  ' 

0 

y'(k)*d/2 

0 

z ' (k)+d/2 
- DE'N’2 

X’ (k)+d/2 

— mrs 

0 

y '  (k)-d/2 

— DETT3 

0 

z'  (k)+d/2 

— mrr~ 

x '  (k)+d/2 
- DEMIT — 

0 

y ' (k)+d/2 

— 

0 

z '  (k)-d/2 

- DENT” 

(4.3) 


DENI  =  [(x'(k)+d/2)2+(y,(k)+d/2)2+(z,(k)+d/2)2]1/2 


DEN2  =  C(x'(k)-d/2)2+(y'(k)+d/2)2+(z,(k)+d/2)2]1/2 
DEN  3  =  [(x'(k)+d/2)2  +  (yf (k) -d/2 )2+( z ' (k)+d/2)2]1/2 
DEN 4  s  [(x'(k)+d/2)2+(yr(k)+d/2)2+(z’(k)-d/2)2]1/2 


The  torpedo  dynamics  used  for  the  tracking  problem  are 

2 

assumed  to  be  1/s  with  estimations  on  five  states  x 
position,  x  velocity,  y  position,  y  velocity  and  z  position 
(height  of  torpedo  above  hydrophone  array). 

The  means  of  the  random  excitation  and  random  noise  are 


assumed  to  be  zero,  i.e., 


E[w(k)]  =  0 


E[v(k) ]  =  0 


Four  measurements  are  taken  every  1.31  seconds,  which  is 

2 

one  time  slot,  and  with  this  sampling  time  the  1/s  plant 
has  state  transition,  (PHI)  and  gamma,  (r)  matrices  equal 
to : 


The  *  matrix,  Q  matrix,  R  matrix,  and  H  matrix  are  then 
used  in  the  Kalman  filter  equations  (3.^). 
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3.  THE  SEQUENTIAL  EXTENDED  KALMAN  FILTER 

In  the  sequential  approach,  the  basic  Kalman  filter 

equations  (3.4)  must  be  modified.  Calculations  are 

performed  on  each  of  the  four  independent  transit  times  in 

the  following  order:  T  ,  T„,  T  and  T  for  each  1.31  second 

c  x  y  z 

time  slot.  The  estimate  of  the  states  x(k/k),  based  on  one 
transit  time  measurement  are  used  as  the  prediction  x(k/k-1) 
for  the  calculations  on  the  next  measurement.  Thus  for  the 
first  time  measurement  T_  only  the  first  row  of  the 
linearizing  H  matrix  is  calculated. 

Next  the  first  gain  column  corresponding  to  the  first 
time  measurement  T  i3  calculated  by 


Gicol 


P(k/k-1 )  H 


irow 


^irow 


R. 


ii 


(4.6) 


where  i  =  1  to  4  corresponding  to  the  four  measured  transit 
times.  Thus,  the  first  row  of  the  H  matrix  is  used  to 
calculate  the  first  column  of  the  gain  matrix  with  both 
corresponding  to  the  first  measured  time  Tc» 

Next,  an  estimate  of  the  particular  observation  time  is 
calculated  using  equation  (3.4f)  evaluated  at  the  predicted 
state  x.(k/k-1 ) . 

The  difference  between  observed  transit  time  and  the 
estimated  transit  times  forms  the  residual  which  is  used  in 
the  estimate  equation 
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x.  =  x(k/k-1)  +  G.  ,  [Residual]  (4.7) 

1  ICO  l 

This  equation  gives  an  estimate  of  the  states  based  on  one 
of  the  four  time  measurements. 

Next,  covariance  of  error  is  calculated  based  on  one 
measurement  by 


=  [I  - 


^icol  ^irow^  ^i-1 


where 


(4.3) 


I  s  identity  matrix 

P^  =  the  covariance  matrix  calculated  from  the 

previous  transit  time  measurement  or  if  i  =  1 , 
the  prediction  P(k/k-1). 


After  the  first  iteration,  x^  becomes  £(k/k-1)  and  P^ 
becomes  P(k/k-1 )  for  the  second  iteration  which  calculates 
the  estimate  of  the  3tates  based  on  the  second  measurement 
T 

V 

After  four  iterations  (k  =  4),  x^  becomes  the  estimate 
for  the  time  slot  x(k/k)  and  P^  becomes  the  covariance  error 
P(k/k) . 

The  predictions  for  the  next  time  slot  are  calculated 


using  equations  (3.4a)  and  (3.4d).  This  process  is  repeated 
for  each  time  slot. 


C.  OPTIMAL  SMOOTHING  PROCESS 

During  the  running  of  the  Extended  Kalman  filter  and 

Smoothing  routine,  after  the  forward  filter  pass  for  each 

\ 

time  slot  (except  the  first),  the  smoothing  subroutine  is 
called.  By  using  the  present  and  previous  filtered  estimate 
of  jc ( t )  ,  a  smoothed  estimate  of  previous  jx(t)  is  calculated. 
This  process  is  repeated  for  each  past  time  slot. 

Solution  of  the  equations  (3.5)  proceeds  as  follows:  As 
an  example,  and  because  it  is  slightly  easier  to  see  when 
actual  times  are  used,  suppose  N  =  30. x  On  the  forward 
filter  pass,  the  values  £(k/k),  2(k/k-1),  £(k/k),  and 
£(k/k-1 )  would  be  computed  and  stored.  On  the  final 
iteration  of  the  forward  pass,  with  k  =  N  =  30, 

2(30/30)  s  x(30/29)  +  G(30)  [z(30)  -H  x(30/29)l 

i.e.,  we  have  computed  and  stored  2(30/30). 

Now,  the  smoothing  process  starts  in  the  reverse 
direction.  Decrement  k  to  k  =  N  -  1  s  29,  then 

2(29/30)  =  £( 29/29 )  +  A(29)  Cx(30/30)  -  x(30/29)] 
stored  stored  stored 

and  A(29)  =  £(29/29 )  ±T  P(30/29)-1 

stored  stored 
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Let  k  =  N  -  2  =  28,  then 


£(28/30)  = 

and  A (28)  = 

Also,  for  each 
JP  ( 29/30 )  = 

£(28/30)  = 


£(28/28)  +  A ( 28 )  [£(29/30)  -  £(29/28) 

stored  computed  stored 

last 

iteration 

£(28/28 )  *T  £(29/28 ) "1 
stored  stored 

of  the  two  preceding  iterations, 

£(29/29)  +  A(29)[£(30/30)  -  £(30/29)] 
stored  computed  stored  stored 

£(28/28)  +  A ( 28 ) [£( 29/30 )  -  £(29/28)] 


AT ( 29 ) 
computed 

AT(28) 


stored  computed  stored 


stored  computed 


V.  TESTING  AND  SIMULATION 


A.  DESCRIPTION 

The  sequential  Extended  Kalman  Filter  and  Smoothing 
routine  is  tested  using  simulated  torpedo  tracks.  A  variety 
of  track  scenarios  were  produced  to  test  the  filter  and 
smoothing  performance  during  single  and  multiple  arrays 
tracking . 

Computer  generated  tracks  were  tested  in  the  first 
series  of  straight  running,  constant  depth  and  constant 
velocity  torpedoes.  A  variety  of  track  scenarios  were  used 
transiting  through  multiple  quadrants  including: 

1.  Crossing  north  of  the  array. 

2.  Crossing  diagonally  through  the  array. 

The  next  series  of  tests  demonstrates  the  ability  of  the 
filter  to  track  through  the  areas  of  multiple  arrays 
including : 

1.  Crossing  above  the  arrays. 

2.  Crossing  diagonally  through  the  arrays. 

All  runs  were  made  with  a  variety  of  initialization 
errors  in  position  and  velocity. 

Zero  mean  Gaussian  noi3e  is  added  to  corrupt  the 
observed  transit  times  for  all  runs. 


B.  THE  GATING  SCHEME 


1  * 

! 


* 
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The  operation  of  the  filter  may  be  adversely  affected  by 
large  measurement  noise.  One  error  of  a  relatively  large 
magnitude  could  invalidate  the  filtered  output  for  many 
subsequent  time  slots.  Before  random  measurement  noise  and 
random  excitations  could  be  added  to  the  observed  times  for 
testing,  a  form  of  protection  was  designed  to  guard  against 
catastrophic  failure.  This  protection  is  provided  by 
establishing  limits  of  acceptability  for  each  of  the 
measurements . 

Measurement  errors  can  occur  because  of  many  factors 
including  an  error  in  the  transit  time  of  the  acoustic  pulse 
primarily  due  to  the  receipt  of  multipath  signals  that  have 
bounced  off  the  surface,  bottom  or  different  density  layers. 

A  three-sigma  gate  was  designed  using  the  covariance  of 
measurement  noise  (R)  and  the  covariance  of  estimation  error 
(P (k/k) ) . 

For  each  calculation  of  a  state  estimate  (x(k/k)),  the 
largest  positional  covariance  of  error  was  used,  either  x,  y 
or  2,  and  converted  to  time  in  seconds  using  the  average 
velocity  of  sound  in  water  for  Dabo'o  3ay,  4360  ft/sec.  The 
gate  then  was  written  for  each  time  measurement  i  =  1  to  4: 


GATE  = 


P(k/k) 


largest 


(4360. )' 


+  R 


ii 
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The  -late  expands  or  decreases  depending  on  the  confidence 
level  of  the  position  estimate  and  the  transit  time.  If 
ZDIFF,  which  is  the  difference  between  the  actual  transit 
time  received  and  the  predicted  transit  time  to  a  particular 
hydrophone,  exceeds  the  gate,  the  measurement  is  considered 
unacceptable  and  the  filter  gain  is  set  to  zero  causing  the 
filter  to  ignore  the  data  and  take  the  prediction  of  the 
states  as  the  estimate 

)t(k/k)  =  )c(k/k-1 ) 

An  invalid  time  measurement  zeros  only  the  gain  column  for 
that  particular  hydrophone  causing  only  that  hydrophone’s 
data  to  be  ignored . 

C.  MULTIPLE  ARRAY  TRACKING 

Initial  tests  were  performed  on  tracks  in  he  ares  of 
one  array.  In  order  to  more  closely  simulate  a  typical  run 
on  the  range,  a  scheme  was  designed  to  track  the  torpedo 
through  multiple  arrays. 

First,  a  coordinate  system  is  defined  as  shown  in 
Figure  3.  The  center  of  the  coordinate  system  is  geographi¬ 
cally  near  the  entrance  to  Dabob  Bay  in  the  simulation. 

Array  number  5  is  the  closest  array  to  be  coordinate  center. 
In  the  simulation  array  1  is  at  36,000  feet  from  coordinate 
center  and  array  6  is  6,000  feet.  The  C  hydrophone  is 
assumed  to  be  the  axis  location  of  each  array.  Then  each  X 
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Coordinate  System  for  Multiple  Array  Tracking 
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Figure  3 
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position  for  the  X  hydrophone  in  each  array  is  XQ  +  30,  each 
Y  position  for  the  Y  hydrophone  is  Y.  +  30,  and  each  Z 
position  for  the  Z  hydrophone  is  ZQ  +  30.  These  72 
positions,  an  XYZ  position  for  each  of  4  hydrophones  in  6 
arrays,  were  placed  into  a  6  x  12  matrix  HYDRO  and 
referenced  throughout  the  routine. 

The  geometry  centered  on  each  array  is  taken  out  of  the 
problem  and  the  target  position  is  based  on  a  central 
reference . 

The  non-linear  time  equation  becomes 

T  =  1/VEL  fix  -  xQ)2  ♦  (y  -  yQ)2  ♦  (z  -  zQ)2 

where  xQ ,  yQ,  or  zQ  is  the  position  of  a  particular  hydro¬ 
phone  and  array  being  used. 

The  decision  parameter  used  to  determine  the  switching 
from  array  to  array  is  a  straight  handoff.  If  the  predicted 
x  position  is  greater  than  3,000  feet  from  the  array  in  use, 
then  an  index  (18)  is  incremented  and  the  next  row  of  HYDRO 
is  implemented.  This  placed  into  the  routine  the  x,  y  and  z 
positions  of  the  hydrophones  in  the  next  array.  The  handoff 
can  easily  be  utilized  in  real  range  operations,  as  the 
transit  times  from  adjacent  arrays  are  present  at  the 
computer  for  a  particular  time  slot.  For  simulation,  it  is 
assumed  that  in  all  the  arrays  each  axis  pointed  in  the  same 
direction.  In  actual  range  operations  each  array  is  tilted 
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about  both  the  X  and  Y  axis.  Since  the  true  transit  times 
are  derived  in  a  tilted  coordinate  frame,  the  filter’s 
estimate  of  transit  time  must  also  be  calculated  in  a  tilted 
coordinate  frame.  The  tilt  angle  measurements  along  with 
the  level  rectangular  coordinates  of  the  array  with  respect 
to  the  central  rectangular  coordinate  system  can  be  input 
into  the  matrix  HYDRO  to  rotate  the  coordinates  of  each 
hydrophone  in  the  array. 


VI.  TEST  RESULTS 


A.  SERIES  ONE 

Figure  5  shows  the  true  trajectory  of  the  torpedo  in  the 
horizontal  X-Y  plane  during  a  straight  run  through  single 
array.  Torpedo  velocity  is  50  knots  in  the  x-direction. 
Initial  position  errors  are  set  to  25  feet  for  X  and  Y. 
Velocity  errors  are  set  to  zero.  Figures  6,  7  and  8  depict 
the  position  errors  for  both  Kalman  filter  and  Smoothing. 
Measurement  noise  is  added  to  all  runs.  The  steady  state  X 
and  Y  position  errors  ranged  between  -6  and  +9  feet  through¬ 
out  the  trajectory  for  Kalman  filter  and  -2  and  +4  feet  for 
smoothing.  The  position  errors  are  computed  by  subtracting 
the  filter  position  estimate,  x(k/k),  for  Kalman  filter  and 
x(k/N),  for  smoothing,  from  the  computer  generated  true 
position  for  each  time  slot.  Figures  9,  10,  11,  12,  and  13 
depict  the  mean  square  estimation  errors  of  states.  These 
estimation  errors  are  obtained  by  taking  the  appropriate 
diagonal  terms  of  the  covariance  matrix  and  smoothed 
covariance  matrix. 

8.  SERIES  TWO 

Figure  14  shows  the  true  trajectory  of  the  torpedo  in 
the  horizontal  X-Y  plane,  during  a  crossing  run  through 
single  array.  Torpedo  velocity  is  40  knots  in  X-direction 
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and  25  knots  in  Y-direction.  The  torpedo  depth  is 
maintained  at  300  feet.  Figures  15,  16,  and  17  depict  the 
position  errors.  Since  the  initial  position  errors  are  set 
to  zero,  the  position  errors  ranged  between  -3  and  +4  feet. 
Figures  18,  19,  20,  21,  and  22  depict  the  mean  square 
estimation  errors  of  states. 

C.  SERIES  THREE 

Figure  23  shows  the  true  trajectory  of  the  torpedo  in 
the  horizontal  X-Y  plane,  during  a  straight  run  through 
multiple  array.  Because  of  storage  problem  of  the  computer, 
the  runs  through  the  multiple  array  were  made  for  190  time 
slots.  Torpedo  velocity  is  50  knots  in  X-direction  and  the 
depth  is  100  feet.  Figures  24,  25,  and  26  show  the  position 
errors  ranged  between  -6  and  17  feet.  Figures  27,  28,  29, 
30,  and  31  depict  the  mean  square  estimation  errors  of 
states.  Figure  32  shows  the  error  ellipsoids  superimposed 
at  every  eighteenth  observation.  The  error  ellipsoids  are 
expanded  to  twenty-five  times  their  true  value  in  order  that 
they  may  be  3een .  The  error  ellipsoids  provide  a  geometric 
interpretation  of  the  behavior  of  the  estimator.  Before  the 
hand-off  point,  at  the  ninetyth  time  slot,  the  major  axis 
rotation  of  the  error  ellipsoid  and  magnitude  of  the  axis 
were  -16.98  degrees  and  43.28  feet,  respectively.  After  the 
hand-off  point  major  axis  rotation  became  25.3  degrees,  and 
its  magnitude  became  1.6  feet.  When  the  magnitude  of  an 
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axis  of  the  ellipsoid  decreases,  the  conclusion  is  that  the 
error  in  the  estimate  decreases,  because  the  observation 
from  the  new  array  has  an  error  covariance  ellipse  rotated 
over  40  degrees  from  the  present  covariance  (-16.94  degree) 
of  the  estimate.  The  ellipsoids  are  extremely  narrow.  When 
combined  the  resultant  covariance  is  reduced  greatly. 

Figure  4  depicts  the  result  of  reduction  and  rotation  of  the 
ellipsoids.  Figure  33  shows  the  error  ellipsoids  before  and 
after  the  hand-off  point. 


Figure  4.  Result  of  Rotation  and  Reduction  of  the 
Error  Ellipsoids 
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D.  SERIES  FOUR 

Figure  34  shows  the  true  trajectory  of  the  torpedo  in 
the  horizontal  X-Y  plane,  during  a  crossing  run  through 
multiple  array.  Torpedo  velocity  is  50  knots  in  X-direction 
and  40  knots  in  Y-direction.  The  torpedo  depth  is  main¬ 
tained  at  300  feet.  Initial  position  errors  are  set  to  25 
feet  for  X  and  Y  and  initial  velocity  errors  are  set  to  5 
knots.  Figures  35,  26,  and  37  show  the  position  and  depth 
errors.  Since  the  initial  position  and  velocity  errors  are 
set  to  25  feet  and  5  knots,  the  big  position  errors  were 
taken  at  the  beginning  of  the  run.  These  values  were 
ignored  from  the  figures  in  order  to  see  clearly  to  the  rest 
of  the  run.  Figures  38,  39,  40,  41,  and  42  show  the  mean 
square  estimation  errors  of  states  for  both  filtered  and 


smoothed . 


VII.  CONCLUSIONS 


The  sequential  Extended  Kalman  Filter  and  Smoothing 
satisfactorily  provided  real  time  estimates  of  torpedo 
position  and  depth.  The  average  of  steady  state  position 
and  depth  errors  ranged  between  3  and  1  feet  for  torpedo 
tracks  within  the  specified  radial  tracking  range  after 
Kalman  filter.  These  errors  had  a  range  of  around  1  foot 
after  smoothing. 

The  filter  performance  was  dependent  on  system  noise  and 
the  distance  the  torpedo  was  from  the  hydrophone  array  and 
the  smoothed  estimates  of  states  were  better  than  or  equal 
to  the  filter  estimates. 

Implementation  at  the  range  computer  facilities  can  be 
accomplished  by  real  time  Kalman  Filtering  and  post  run 
Smoothing  of  the  raw  time  data.  Future  tests  should  include 
evaluating  filter  performance  using  trajectories  generated 
from  actual  torpedo  runs  on  the  Dabob  test  range.  These 
test3  would  verify  the  adequacy  of  the  noise  model  in  the 
filter  and  the  ability  of  the  software  to  edit  erroneous 
transit  time  measurements. 

The  rotation  and  reduction  of  the  error  ellipsoids 
(i.e.,  the  filter  error  covariance)  was  most  instructive  and 
gave  much  insight  into  the  performance  of  the  filter. 
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Note:  Hydrophone  array  is  located 


Figure  5.  True  Trajectory  of  the  Torpedo  During  a  Straight  Run  Through 
Single  Array  (Right  to  Left) 


After  Kalman  Filter 


in  Torpedo  Y-Position  During  a  Straight  Run  Through 


gure  8.  Error  in  Torpedo  Z-Position  During  a  Straight  Run  Through 
Single  Array 


Istimation  Error  (ft.  )  in  X-Position  During 
Through  Single  Array 


Figure  11.  Mean  Square  Estimation  Error  (ft.  )  in  Y-Position  During 
a  Straight  Run  Through  Single  Array 


Estimation  Error  (ft.  /sec.  )  in  Y-Velocity 
aight  Run  Through  Single  Array 


X-Position  (ft.)  Results  of  Straight  Run  Through  Single  Array 
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TABLE  2 

Y-Position  (ft.)  Results  of  Straight  Run  Through  Single  Array 
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Position  (ft.)  Results  of  Straight  Run  Through  Single  Array 
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Figure  15.  Error  in  Torpedo  X-Position  During  a  Straight  Run  Through  Single 


After  Kalman  Filter 


Error  in  Torpedo  Y-Position  During  a  Straight  Run  Through  Singl 


Error  in  Torpedo  Z-Position  During  a  Straight  Run  Through  Single 


>uoo 
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Figure  19.  Mean  Square  Estimation  Error  (ft.  /sec.  )  in  X-Velocity  During 
a  Straight  Run  Through  Single  Array 


Estimation  Error  (ft.  )  in  Z-Position  During 
Through  Single  Array 


Figure  23.  True  Trajectory  of  the  Torpedo  During  a  Straight  Run  Through 
Multiple  Array 


Multip 


Figure  25.  Error  in  Torpedo  Y-Position  During  a  Straight  Run  Through 
Multiple  Array 


Figure  26.  Error  in  Torpedo  Z-Position  During  a  Straight  Run  Through 
Multiple  Array 


Mean  Square  Estimation  Error  (ft.  /sec.  )  in  X-Velocity  During 
a  Straight  Run  Through  Multiple  Array 


Mean  Square  Estimation  Error  (ft.  /sec.  )  in  Y-Velocity  During 
a  Straight  Run  Through  Multiple  Array 
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Figure  32.  Error  Ellipsoids  Presented  on  Every  Eighteenth  Observation 
During  a  Straight  Run  Through  Multiple  Array 


Figure  36.  Errors  in  Torpedo  Y-Position  During  a  Straight  Run  Through 
Multiple  Arrays 


an  Square  Estimation  Error  (ft.  /sec,  )  in  X-Velocity  During 
Straight  Run  Through  Multiple  Arrays 


Figure  41.  Mean  Square  Estimation  Error  (ft.  /sec.  )  in  Y-Velocity 
During  a  Straight  Run  Through  Multiple  Arrays 


0001 


Figure  42.  Mean  Square  Estimation  Error  (ft.  )  in  Z-Position  During 
a  Straight  Run  Through  Multiple  Arrays 


APPENDIX  A 


PROGRAM  DESCRIPTION  AND  FEATURES 


The  sequential  Extended  Kalman  Filter  and  Smoothing 
routine  used  for  torpedo  tracking  is  modularized  for  ease  of 
implementation.  The  program  is  general  in  nature  and  many 
of  the  parameters  of  the  filter  are  variable  including: 

a.  The  number  of  states  in  the  filter  —  N 

b.  The  number  of  random  forcing  functions  —  M 

c.  The  number  of  measurements  —  JS 

d.  Number  of  time  slots  —  JTIME 

The  constant  matrices  PHI,  R,  COVW,  and  GAMMA  are 
initialized  in  the  beginning  of  the  program  using  data 
statements.  The  filter  is  initialized  with  P(1/0)  and 
x(1/0)  (initial  covariance  of  estimation  error  and  states) 
using  subroutine  INIT.  The  first  estimate  is  at  time  1  and 
continues  until  ITIME  =  JTIME  +  1.  True  measurement  times 
(ZI)  are  computed  using  either  subroutines  TRAJEC  or  TRAJC3f 
depending  on  whether  single  array  or  multiple  array  tracking 
is  implemented.  Either  subroutine  will  compute  four 
measurement  times  ( Tc ,  Tx,  Ty,  Tz )  for  each  time  slot.  The 
measurement  times  are  corrupted  by  zero-mean,  white  Gaussian 
noise  using  the  IBM-3033  subroutine  GGNML.  For  each  of  the 
four  time  measurements  the  corresponding  row  of  the 
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linearizing  H  matrix  is  calculated  using  either  subroutine 
CHROW  or  CHR0W3i  depending  on  whether  single  array  or 
multiple  array  tracking  is  used.  The  corresponding  gain 
matrix  column  GI  is  then  found.  These  row  and  column  values 
are  utilized  in  forming  the  covariance  of  estimation  error. 
PI,  for  that  particular  time  measurement.  Next  the  estimate 
of  the  observation  time  ZHAT  from  that  particular  hydrophone 
is  formed  using  the  subroutine  CZHAT  or  CZHAT3 ,  depending  on 
whether  single  or  multiple  array  tracking  is  implemented. 

The  residual  ZDIFF(I)  s  ZIC(I)  -  ZHAT,  is  then  calculated. 
Finally  the  estimate  of  the  states  XI  based  on  one  time 
measurement  is  calculated  and  the  process  is  repeated  for 
the  next  measurement.  After  four  iterations,  XI  becomes  the 
state  estimate  XKK  and  PI  becomes  the  updated  covariance  of 
estimation  error  PKK,  and  the  predictions  of  the  states  and 
covariances  XKKM1  and  PKXM1  are  formed.  Finally,  for  each 
time  slot  (except  the  first)  smoothed  state  estimates,  XXKS , 
and  covariances,  PKKS,  are  formed  using  the  subroutine 
SMOOTH.  PLOTP  is  used  to  generate  line  printer  plots  and 
PLOTG  is  used  to  generate  VERSATEC  plots. 

A.  PROGRAM  SUBROUTINES 

A  brief  description  of  the  subroutines  are  described 
below : 

1.  TRAJEC  --  This  subroutine  develops  the  torpedo 
trajectory  which  is  used  as  truth  data  for  the  filter.  The 


77 


subroutine  outputs  true  transit  times,  21(1) ,  and  the  x,  y, 

2  positions,  TD(I),  of  the  torpedo  for  each  time  slot.  THe 
subroutine  is  used  during  single  array  tracking. 

2.  TRAJC3  --  This  subroutine  performs  the  same  function 
as  TRAJEC  but  is  used  only  during  multiple  array  tracking. 

3.  INIT  —  This  subroutine  generates  the  initial  state 
vector  x ( 0/  —  1 )  and  initial  covariance  matrix  P ( 0 /  —  1 ) . 

4.  CHROW  --  This  subroutine  computes  the  appropriate 

row  of  the  linearizing  H  matrix.  Each  row  corresponds  to 

one  of  the  four  transit  time  measurements,  T„ ,  T  ,  T  ,  T  . 

c  x  ’  y 1  z 

This  subroutine  is  used  during  single  array  tracking. 

5.  CHR0W3  —  This  subroutine  performs  the  3ame 
functions  as  CHROW  but  is  used  only  during  multiple  array 
tracking. 

6.  CZHAT  —  This  subroutine  computes  the  estimated 
transit  times  for  the  filter.  Four  transit  times,  ZHAT,  are 
calculated  corresponding  to  each  of  the  four  true  transit 
times  ZI(I).  This  subroutine  is  used  during  single  array 
tracking . 

7.  CZHAT3  —  Subroutine  performs  3ame  functions  as 
CZHAT  however  it  is  U3ed  only  during  multiple  array 
tracking . 

8.  QFIND  —  This  subroutine  develops  an  adaptive  Q 
matrix  which  is  a  function  of  the  torpedo  velocity.  Three 
input  variables  defined  in  a  data  statement  at  the  beginning 
of  the  program  can  be  adjusted: 
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aa .  SIGACC  --  Maximum  expected  horizontal 
acceleration  of  the  torpedo. 

bb.  SIGDIV  —  Maximum  expected  change  in  vertical 
velocity . 

cc .  SIGCC  —  Maximum  expected  turn  rate  of  the 
torpedo  in  the  horizontal  plane. 

The  values  listed  in  the  program  were  used  and  kept 
constant  during  the  simulation  tests.  If  the  user  desires 
not  to  use  the  adaptive  Q  subroutine,  software  code  is 
provided  at  the  beginning  of  the  program  to  calculate  a 
constant  Q  matrix . 

9.  GGNML  —  This  is  an  IBM-3033  subroutine  contained  in 
the  IMSL  library.  The  routine  generates  zero  mean  white 
Gaussian  noise  with  an  RMS  value  normalized  to  1.  The  main 
program  scales  the  noise  and  adds  it  to  the  transit  time 
measurements . 

10.  PLOTP  —  This  is  an  I3M-3033  subroutine  used  to 
generate  the  line  printer  plots.  Information  on  this 
subroutine  can  be  obtained  from  the  IMSL  library. 

11.  PLOTG  —  This  is  an  I3M-3033  subroutine  used  to 
generate  the  VERSATEC  plots.  Information  on  this  subroutine 
can  be  obtained  from  the  IMSL  library. 

12.  SMOOTH  --  This  subroutine  computes  the  smoothed 


state  estimates  and  covariances. 


B.  UTILITY  PROGRAMS 

These  subroutines  were  designed  to  be  used  for 
repetitive  matrix  and  vector  manipulations: 

1.  PROD  —  multiplying  two  matrices 

2.  MMULT  —  multiplying  a  matrix  and  a  vector 

3.  VMULT  —  multiplying  two  vectors 

4.  TRANS  —  transposing  a  matrix 

5.  ADD  —  adding  two  matrices 

6.  SUB  —  subtracting  two  matrices 

7.  RECIP  —  inversing  a  matrix 


APPENDIX  3 


SEQUENTIAL  EXTENDED  KALMAN  FILTER  AMD 
SMOOTHING  PROGRAM  LISTING 
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